/**
 * @file Data.cpp
 * @author Giulio Romualdi
 * @copyright Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
 * @date 2018
 */

// std
#include <iostream>

// OsqpSolver
#include <OsqpSolver/Data.hpp>

OsqpSolver::Data::Data()
	: m_isNumberOfVariablesSet(false),
	  m_isNumberOfConstraintsSet(false),
	  m_isHessianMatrixSet(false),
	  m_isGradientSet(false),
	  m_isLinearConstraintsMatrixSet(false),
	  m_isLowerBoundSet(false),
	  m_isUpperBoundSet(false)
{
	m_data = (OSQPData *)c_malloc(sizeof(OSQPData));
	m_data->P = nullptr;
	m_data->A = nullptr;
}

OsqpSolver::Data::Data(int n, int m)
	: m_isNumberOfVariablesSet(true),
	  m_isNumberOfConstraintsSet(true),
	  m_isHessianMatrixSet(false),
	  m_isGradientSet(false),
	  m_isLinearConstraintsMatrixSet(false),
	  m_isLowerBoundSet(false),
	  m_isUpperBoundSet(false)
{
	m_data = (OSQPData *)c_malloc(sizeof(OSQPData));
	m_data->P = nullptr;
	m_data->A = nullptr;

	setNumberOfVariables(n);
	setNumberOfConstraints(m);
}

void OsqpSolver::Data::clearHessianMatrix()
{
	if (m_isHessianMatrixSet)
	{
		m_isHessianMatrixSet = false;
		csc_spfree(m_data->P);
		m_data->P = nullptr;
	}
}

void OsqpSolver::Data::clearLinearConstraintsMatrix()
{
	if (m_isLinearConstraintsMatrixSet)
	{
		m_isLinearConstraintsMatrixSet = false;
		csc_spfree(m_data->A);
		m_data->A = nullptr;
	}
}

OsqpSolver::Data::~Data()
{
	clearHessianMatrix();
	clearLinearConstraintsMatrix();
	c_free(m_data);
}

void OsqpSolver::Data::setNumberOfVariables(int n)
{
	m_isNumberOfVariablesSet = true;
	m_data->n = n;
}

void OsqpSolver::Data::setNumberOfConstraints(int m)
{
	m_isNumberOfConstraintsSet = true;
	m_data->m = m;
}

OSQPData *const &OsqpSolver::Data::getData() const
{
	return m_data;
}

bool OsqpSolver::Data::isSet() const
{
	return m_isNumberOfVariablesSet &&
		   m_isNumberOfConstraintsSet &&
		   m_isHessianMatrixSet &&
		   m_isGradientSet &&
		   m_isLinearConstraintsMatrixSet &&
		   m_isLowerBoundSet &&
		   m_isUpperBoundSet;
}

bool OsqpSolver::Data::setGradient(Eigen::Ref<Eigen::Matrix<c_float, Eigen::Dynamic, 1>> gradient)
{
	if (gradient.rows() != m_data->n)
	{
		std::cerr << "[OsqpSolver::Data::setGradient] The size of the gradient must be equal to the number of the variables."
				  << std::endl;
		return false;
	}
	m_isGradientSet = true;
	m_data->q = gradient.data();
	return true;
}

bool OsqpSolver::Data::setLowerBound(Eigen::Ref<Eigen::Matrix<c_float, Eigen::Dynamic, 1>> lowerBound)
{
	if (lowerBound.rows() != m_data->m)
	{
		std::cerr << "[OsqpSolver::Data::setLowerBound] The size of the lower bound must be equal to the number of the variables."
				  << std::endl;
		return false;
	}
	m_isLowerBoundSet = true;
	m_data->l = lowerBound.data();
	return true;
}

bool OsqpSolver::Data::setUpperBound(Eigen::Ref<Eigen::Matrix<c_float, Eigen::Dynamic, 1>> upperBound)
{
	if (upperBound.rows() != m_data->m)
	{
		std::cerr << "[OsqpSolver::Data::setLowerBound] The size of the upper bound must be equal to the number of the variables."
				  << std::endl;
		return false;
	}
	m_isUpperBoundSet = true;
	m_data->u = upperBound.data();
	return true;
}
